#include "GenericTypeDefs.h"
#include "uart.h"
#include "tripod.h"

// Servo: [Tripod][Servo][Leg]
/*const*/ char* servos[2][3][3] =	{
									{ // [0] : Tripod A
										{"#24", "#4", "#16"}, // [0] : Horizontal
										{"#25", "#5", "#17"}, // [1] : Vertical, Superior
										{"#26", "#6", "#18"}  // [2] : Vertical, Inferior
									},
									{ // [1] : Tripod B
										{"#8", "#0", "#20"}, // [0]: Horizontal
										{"#9", "#1", "#21"}, // [1]: Vertical, Superior
										{"#10", "#2", "#22"} // [2]: Vertical, Inferior
									}
								};
// Estado do servo: [Tripod][Perna][Servo][Estado]
/*const */char* servoState[2][3][3][5] = {
										{ // Tripod A
											{ // Leg1 // Front, back
												{"P1500","P1800","P1200","P1425","P1637"}, // Horizontal
												{"P1500","P1200","","P1526","P1479"}, // Vertical Superior
												{"P1500","P1200","","P1638","P1422"} // Vertical Inferior
											}, // EndLeg1
											{ // Leg2
												{"P1500","P1800","P1200","P1689","P1311"}, // Horizontal
												{"P1500","P1800","","P1499","P1499"}, // Vertical Superior
												{"P1500","P1800","","P1479","P1479"} // Vertical Inferior
											}, // EndLeg2
											{ // Leg3
												{"P1500","P1800","P1200","P1363","P1575"}, // Horizontal
												{"P1500","P1200","","P1479","P1526"}, // Vertical Superior
												{"P1500","P1200","","P1422","P1638"} // Vertical Inferior
											} // EndLeg3
										}, // EndTripodA
										{ // Tripod B
											{ // Leg1 // Front, Back
												{"P1500","P1800","P1200","P1575","P1363"}, // Horizontal
												{"P1500","P1800","","P1474","P1521"}, // Vertical Superior
												{"P1500","P1800","","P1362","P1578"} // Vertical Inferior
											}, // EndLeg1
											{ // Leg2
												{"P1500","P1800","P1200","P1637","P1425"}, // Horizontal
												{"P1500","P1800","","P1521","P1474"}, // Vertical Superior
												{"P1500","P1800","","P1578","P1362"} // Vertical Inferior
											}, // EndLeg2
											{ // Leg3
												{"P1500","P1800","P1200","P1311","P1689"}, // Horizontal
												{"P1500","P1200","","P1501","P1501"}, // Vertical Superior
												{"P1500","P1200","","P1521","P1521"} // Vertical Inferior
											} // EndLeg3
										} // EndTripodB
									};

/*const */char* speed = "T500";

void setState(int tripodAState, int tripodBState){
	int not_a = 0, not_b = 0;
	//Define Tripod A State

	if(tripodAState == lowCenter){
		defineState(tripodA, verticalSup, lowPos);
		defineState(tripodA, verticalInf, lowPos);
		defineState(tripodA, horizontal, centerPos);
	} else if(tripodAState == highCenter){
		defineState(tripodA, verticalSup, highPos);
		defineState(tripodA, verticalInf, highPos);
		defineState(tripodA, horizontal, centerPos);
	} else if(tripodAState == lowRight){
		defineState(tripodA, verticalSup, lowPos);
		defineState(tripodA, verticalInf, lowPos);
		defineState(tripodA, horizontal, rightPos);
	} else if(tripodAState == lowLeft){
		defineState(tripodA, verticalSup, lowPos);
		defineState(tripodA, verticalInf, lowPos);
		defineState(tripodA, horizontal, leftPos);
	}  else if(tripodAState == lowFront){
		defineState(tripodA, verticalSup, frontPos);
		defineState(tripodA, verticalInf, frontPos);
		defineState(tripodA, horizontal, frontPos);
	}  else if(tripodAState == lowBack){
		defineState(tripodA, verticalSup, backPos);
		defineState(tripodA, verticalInf, backPos);
		defineState(tripodA, horizontal, backPos);
	} else not_a = 1;

	// Define Tripod B State

	if(tripodBState == lowCenter){
		defineState(tripodB, verticalSup, lowPos);
		defineState(tripodB, verticalInf, lowPos);
		defineState(tripodB, horizontal, centerPos);
	} else if(tripodBState == highCenter){
		defineState(tripodB, verticalSup, highPos);
		defineState(tripodB, verticalInf, highPos);
		defineState(tripodB, horizontal, centerPos);
	} else if(tripodBState == lowRight){
		defineState(tripodB, verticalSup, lowPos);
		defineState(tripodB, verticalInf, lowPos);
		defineState(tripodB, horizontal, rightPos);
	} else if(tripodBState == lowLeft){
		defineState(tripodB, verticalSup, lowPos);
		defineState(tripodB, verticalInf, lowPos);
		defineState(tripodB, horizontal, leftPos);
	}  else if(tripodBState == lowFront){
		defineState(tripodB, verticalSup, frontPos);
		defineState(tripodB, verticalInf, frontPos);
		defineState(tripodB, horizontal, frontPos);
	}  else if(tripodBState == lowBack){
		defineState(tripodB, verticalSup, backPos);
		defineState(tripodB, verticalInf, backPos);
		defineState(tripodB, horizontal, backPos);
	} else not_b = 1;

	if(!(not_a && not_b))
		moveIt();
}

void defineLegState(int tripod, int leg, int position){
	int positions[3], tipo;

	switch(position){
		case 	lowCenter: 	{positions[0] = centerPos;	positions[1] = lowPos; 	positions[2] = lowPos;}	break;
		case 	highCenter:	{positions[0] = centerPos;	positions[1] = highPos; 	positions[2] = highPos;}	break;
		case 	lowRight:	{positions[0] = rightPos;	positions[1] = lowPos; 	positions[2] = lowPos;}	break;
		case 	lowLeft: 	{positions[0] = leftPos;	positions[1] = lowPos; 	positions[2] = lowPos;}	break;
		case 	lowFront:	{positions[0] = frontPos;	positions[1] = frontPos; 	positions[2] = frontPos;}	break;
		case 	lowBack: 	{positions[0] = backPos;	positions[1] = backPos; 	positions[2] = backPos;}	break;
		default	: 			{positions[0] = centerPos;	positions[1] = lowPos; 	positions[2] = lowPos;}	break;
	}

	for(tipo = 0; tipo<3; tipo++){
            sendString(servos[tripod][tipo][leg]);
            sendString(" ");
            sendString(servoState[tripod][leg][tipo][positions[tipo]]);
            sendString(" ");
	}

	sendString(speed);
	sendString(" ");
}

void defineState(int tripod, int tipo, int posicao){
	int i;
	for(i = 0; i<3; i++){
		sendString(servos[tripod][tipo][i]);
		sendString(" ");
		sendString(servoState[tripod][i][tipo][posicao]);
		sendString(" ");
	}
	sendString(speed);
	sendString(" ");
}

void defineStateNoTime(int tripod, int tipo, int posicao){
	int i;
	for(i = 0; i<3; i++){
		sendString(servos[tripod][tipo][i]);
		sendString(" ");

		sendString(servoState[tripod][i][tipo][posicao]);
		sendString(" ");
	}
}

void reset(){
	defineStateNoTime(tripodA, verticalSup, lowPos);
	defineStateNoTime(tripodA, verticalInf, lowPos);
	defineStateNoTime(tripodA, horizontal, centerPos);
	defineStateNoTime(tripodB, verticalSup, lowPos);
	defineStateNoTime(tripodB, verticalInf, lowPos);
	defineStateNoTime(tripodB, horizontal, centerPos);
	moveIt();
}

void moveIt(){
	sendString("\r");
}

void setLowPos(){
	defineState(tripodA, verticalSup, lowPos);
	defineState(tripodB, verticalSup, lowPos);
	moveIt();
}
